#include "ros/ros.h"
#include <geometry_msgs/Twist.h>         // 指线速度角速度的消息类型
#include <tf/transform_listener.h>       // 用于坐标转换
#include <nav_msgs/Path.h>                  // 显示历史轨迹和路径
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>
#include <nav_msgs/Odometry.h>           // 用于读取里程计信息
#include <sensor_msgs/LaserScan.h>       // lidar
#include "ag_robot/shnMPCfunction.h"               // MPC解优化
#include <ag_robot/cooperate_game.h>
// #include <Eigen>                    // 矩阵运算
#include <Eigen/Core>                    // 矩阵运算
#include <Eigen/QR>

// inlcude iostream and string libraries
#include <iostream>
#include <map>      // 包含了key-value键值对映射的排序分配容器
#include <math.h>
#include <string>
#include <vector>

constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }

class shnMPCNode
{
    public:
        shnMPCNode();  // 构造函数
        ~shnMPCNode(); // 析构函数
        int get_thread_numbers();   // 获取线程数
        vector<double> mpc_results;  //MPC Output
        void Setting_MPC_test(double _distance, double _degree);
        double speed_from_human;
        double angular_from_human;
        // COOP::COOPFunction _coop_linear;
    
    private:
        ros::NodeHandle _nh;
        ros::Subscriber _sub_odom, _sub_distance, _sub_cmd, _sub_cleantime;
        ros::Publisher _pub_totalcost, _pub_twist, _pub_mpctraj, _pub_mpcdif;
        ros::Timer _timer1;

        nav_msgs::Odometry _odom;
        geometry_msgs::Twist _twist_msg, _sub_twist_cmd;
        geometry_msgs::Twist _robot_twist_cmd;
        nav_msgs::Path _mpc_traj;

        string _map_frame, _odom_frame, _car_frame;
        string _leader_frame;

        MPCFunction _mpc;
        map<string, double> _mpc_params;
        double _mpc_steps, _w_linvel, _w_linvel_d, _w_angvel, _w_angvel_d, _max_angvel, _max_speed, _bound_value;

        double _dt, _w, _speed;
        int _controller_freq, _thread_numbers;
        bool _debug_info, _delay_mode;
        float _closest_distance, _closest_degree;                               // 雷达测量障碍物的最近距离和角度
        double _target1_x, _target1_y, _target2_x, _target2_y;                  // set target A(5, 5) B(5, -5)
        double distanceToA, distanceToB;                                        // 任意时刻Leader与目标的距离

        void odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg);               // 订阅里程计消息回调函数
        void controlLoopCB(const ros::TimerEvent&);                             // 控制回调函数
        void get_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg);   // 订阅雷达消息回调函数
        void read_cmd(const geometry_msgs::Twist::ConstPtr& cmd);               // 读取用户输入的命令
        void clean_time_CB(const std_msgs::Int32::ConstPtr& msg);               // 将当前时间归零
        void Generate_Usr_Command(double linear, double angular);           // 测试用，生成指定指令
        void Generate_Robot_Command(double linear, double angular);     // 测试用，生成指定指令
        // Intention tracking control
        void bci_ctrl();   

        unsigned int idx; 
        double _time_begin;       // 记录开始时间
        double _cur_time;         // 当前时间
        std_msgs::Int32 cwm_flag; // 记录当前状态（到达A、B或者超时）

        geometry_msgs::Twist _mpc_dif; // 记录mpc变化情况
}; 

// 构造函数
shnMPCNode::shnMPCNode()
{
    //Private parameters handler
    ros::NodeHandle pn("~");

    //Parameters for control loop
    pn.param("thread_numbers", _thread_numbers, 2); // number of threads for this ROS node
    pn.param("debug_info", _debug_info, true);
    pn.param("delay_mode", _delay_mode, false);
    pn.param("controller_freq", _controller_freq, 5);
    _dt = double(1.0/_controller_freq); // time step duration dt in s 

    //Parameter for MPC solver
    pn.param("mpc_steps", _mpc_steps, 4.0);
    pn.param("mpc_w_angvel", _w_angvel, 5.0);
    pn.param("mpc_w_angvel_d", _w_angvel_d, 1.0);
    pn.param("mpc_w_linvel", _w_linvel, 100.0);
    pn.param("mpc_w_linvel_d", _w_linvel_d, 1.0);    
    pn.param("max_speed", _max_speed, 0.1);// unit: m/s
    pn.param("max_angvel", _max_angvel, 0.25); // Maximal angvel radian 
    pn.param("mpc_bound_value", _bound_value, 1.0e3); // Bound value for other variables

    //Parameter for topics & Frame name
    pn.param<std::string>("map_frame", _map_frame, "tb3_0/odom" ); //*****for mpc, "odom"
    pn.param<std::string>("odom_frame", _odom_frame, "tb3_0/odom");
    pn.param<std::string>("car_frame", _car_frame, "tb3_0/base_footprint" );
    pn.param<std::string>("leader_frame", _leader_frame, "tb3_0" );

    // set targets
    pn.param("target1_x", _target1_x, 5.0);
    pn.param("target1_y", _target1_y, 5.0);
    pn.param("target2_x", _target2_x, 5.0);
    pn.param("target2_y", _target2_y, -5.0);

    //Display the parameters
    cout << "\n===== Parameters =====" << endl;
    cout << "frequency: "   << _dt << endl;
    cout << "mpc_steps: "   << _mpc_steps << endl;
    cout << "mpc_w_linvel: "   << _w_linvel << endl;
    cout << "mpc_w_linvel_d: "  << _w_linvel_d << endl;    
    cout << "mpc_w_angvel: "   << _w_angvel << endl;
    cout << "mpc_w_angvel_d: "  << _w_angvel_d << endl;
    cout << "mpc_max_angvel: "  << _max_angvel << endl;
    cout << "mpc_max_speed: "  << _max_speed << endl;
    //command from BCI_part:
    speed_from_human = 0.0;
    angular_from_human = 0.0;

    //Publishers and Subscribers
    _sub_odom   = _nh.subscribe(_leader_frame + "/odom", 1, &shnMPCNode::odomCB, this);                   // 订阅里程计消息
    _sub_distance   = _nh.subscribe(_leader_frame + "/scan", 1, &shnMPCNode::get_laser_callback, this);   // 订阅雷达消息

    _sub_cmd = _nh.subscribe("/cmd_user", 1, &shnMPCNode::read_cmd, this); // change 20210113
    // _sub_cleantime = _nh.subscribe("/publishCleanTime", 1, &shnMPCNode::clean_time_CB, this);

    _pub_twist = _nh.advertise<geometry_msgs::Twist>(_leader_frame + "/cmd_vel", 1);    // publish speed msg
    _pub_totalcost  = _nh.advertise<std_msgs::Float32>("/total_cost", 1);               // total_cost
    //_pub_mpctraj = _nh.advertise<nav_msgs::Path>("/mpc_trajectory", 1);   // MPC trajectory output  /  Prediction time domain
    //_pub_mpcdif = _nh.advertise<geometry_msgs::Twist>("/mpc_dif", 1);  // 发布MPC的变化情况
    
    //Timer
    _timer1 = _nh.createTimer(ros::Duration((1.0)/_controller_freq), &shnMPCNode::controlLoopCB, this); // 5Hz //*****mpc

    //Init parameters for MPC object
    _mpc_params["DT"] = _dt;
    _mpc_params["STEPS"]    = _mpc_steps;
    _mpc_params["W_LINVEL"]  = _w_linvel;
    _mpc_params["W_DLINVEL"] = _w_linvel_d;    
    _mpc_params["W_ANGVEL"]  = _w_angvel;
    _mpc_params["W_DANGVEL"] = _w_angvel_d;
    _mpc_params["MAX_ANGVEL"]   = _max_angvel;
    _mpc_params["MAX_SPEED"]   = _max_speed;
    _mpc_params["BOUND"]    = _bound_value;
    _mpc.LoadParams(_mpc_params);

    idx = 0;         // 求解次数

    _w = 0.0;        // 角速度
    _speed = 0.0;    // 速度

    _closest_degree = 0;        // 初始化最近障碍物角度和距离
    _closest_distance = 3.5;

    _twist_msg = geometry_msgs::Twist();
    _mpc_traj = nav_msgs::Path();

    _time_begin = ros::Time::now().toSec();
    // while (_time_begin == 0){
    //     _time_begin = ros::Time::now().toSec();
    // }
    // cout << "time_begin： " << _time_begin << endl;
}
shnMPCNode::~shnMPCNode()
{
    // 析构函数  
}
int shnMPCNode::get_thread_numbers()
{
    return _thread_numbers;
}
// Timer: Control Loop (closed loop nonlinear MPC)
void shnMPCNode::controlLoopCB(const ros::TimerEvent&)
{       
    geometry_msgs::Twist cmd_user = _sub_twist_cmd;
    double curent_time = ros::Time::now().toSec() - _time_begin;
    // cout << "duration: " << curent_time << endl; 
    _mpc_dif.linear.z = 0.0;
    bci_ctrl();
}

void shnMPCNode::bci_ctrl() {
    // input variables
    nav_msgs::Odometry odom = _odom;
    tf::TransformListener listener;
    geometry_msgs::Twist cmd_user = _sub_twist_cmd;
    const double cmd_v = cmd_user.linear.x;       // Linear velocity
    const double cmd_w = cmd_user.angular.z;      // angular velocity

    // user's command
    Eigen::VectorXd vec_cmd(2);
    Eigen::VectorXd robot_cmd(2);
    vec_cmd   << cmd_v , cmd_w;

    robot_cmd << cmd_v , cmd_w;

    _mpc_dif.linear.x = cmd_v;
    _mpc_dif.linear.y = cmd_w;

    // distance and degree 距离障碍物的最近距离和角度
    const double min_distance = _closest_distance;  
    const double min_degree = _closest_degree;  

    // 当前时间
    _cur_time = ros::Time::now().toSec() - _time_begin;

    // 判断条件： 离目标较远， 时间不到5分钟
    if (_cur_time < 300.0) {
        // state
        Eigen::VectorXd state(5);
        state << 0, 0, 0, min_distance, min_degree;

        // Solve MPC Problem
        vector<double> mpc_results = _mpc.Solve(state, vec_cmd, robot_cmd, cmd_v, cmd_w);

        // MPC result (all described in car frame), output = (w)  
        _speed = mpc_results[0];            // Linear velocity
        _w = mpc_results[1];                // radian/sec, angular velocity

        cwm_flag.data = 0;
    } else {
        _speed = 0.0;                       // Linear velocity
        _w = 0.0;                           // radian/sec, angular velocity
        cout << "Time out !!!!!!!" << endl;   
        cwm_flag.data = 3; 
    }

    // publish general cmd_vel
    _twist_msg.linear.x  = _speed; 
    _twist_msg.angular.z = _w;
    std::cout << "_twist_msg.linear.x is :"  << _speed << std::endl;
    std::cout << "_twist_msg.angular.z is :" << _w << std::endl;
    _pub_twist.publish(_twist_msg);


    std_msgs::Float32 mpc_total_cost;
    mpc_total_cost.data = static_cast<float>(_mpc._mpc_totalcost);
    std::cout << "The cost is :" << mpc_total_cost.data << std::endl;
    _pub_totalcost.publish(mpc_total_cost);

}
void shnMPCNode::Setting_MPC_test(double _distance, double _degree)
{
        // user's command
    Eigen::VectorXd vec_cmd(2);
    Eigen::VectorXd robot_cmd(2);
    vec_cmd << 1.0 , 0.00;
    robot_cmd << 1.0,0.00;

      // Update system states: X=[x, y, theta]
    const double px = 0.0; //pose: odom frame
    const double py = 0.0;
    const double theta = 0.0; 
    const double v = 0.0; //twist: body fixed frame 
    const double w = 0.0; //angular_velocity
    // state
    Eigen::VectorXd state(5);
     state << 0, 0, 0, _distance, _degree;

     // Solve MPC Problem
     mpc_results = _mpc.Solve(state, vec_cmd, robot_cmd, 0.5 , 0.25);
}
// CallBack: Update odometry
void shnMPCNode::odomCB(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    _odom = *odomMsg;
}
// 读取用户输入的命令 
void shnMPCNode::read_cmd(const geometry_msgs::Twist::ConstPtr& cmd)
{
    _sub_twist_cmd.linear.x = cmd->linear.x;
    _sub_twist_cmd.angular.z = cmd->angular.z;

}
// 将当前时间清零
void shnMPCNode::clean_time_CB(const std_msgs::Int32::ConstPtr& msg)
{
    int key = msg->data;
    // cout << "key: " << key << endl;
    if (key == 0) {
        _time_begin = ros::Time::now().toSec();
        while (_time_begin == 0){
            _time_begin = ros::Time::now().toSec();
        }
        // cout << "time_begin： " << _time_begin << endl;
    }

}
// 获取雷达最近距离和角度
void shnMPCNode::get_laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    // 获取距离数组
    std::vector<float> ranges=msg->ranges;
    float angle_min1 = msg -> angle_min;
    float angle_max1 = msg -> angle_max;

    // 获取距离数组的长度
    int len_ = ranges.size();

    // 距离数组标准化
    for(int i=0 ; i < len_ ; i++)
    {
        if(isinf(ranges[i]))
        {
            ranges[i] = 3.5;
        }
        else if(isnan(ranges[i]))
        {
            ranges[i] = 0.0;
        }
    }
    // the detla angle of laserscan shn:20240114
    double delta_scan = (angle_max1-angle_min1)/len_;
    // get the angle in front of the vehicle. shn:20240114
    std::vector<float>::const_iterator Start = ranges.begin() + 181;
    std::vector<float>::const_iterator End   = ranges.begin() + 541;

    std::vector<float> Arrs1(Start, End);

    // The least distance 
    auto closest_range= std::min_element(Arrs1.begin(),Arrs1.end());
    float closest_range_ = *(closest_range);

    // auto closest_range_test = std::min_element(ranges.begin(),ranges.end());
    // float closest_range_test_ = *(closest_range_test);
    auto closest_range_seq = closest_range - Arrs1.begin() + 181;
    // auto closest_range_seq = closest_range_test - ranges.begin();
    float closest_degree_rad = angle_min1 + closest_range_seq*delta_scan;

    // // 求最短距离所在位置(角度rad)
    // if(closest_range_neg_ <= closest_range_pos_)
    // {
    //     closest_range_ = closest_range_neg_;
    //     auto closest_range_seq = closest_range_neg - Arrs1.begin();
    //     closest_degree_rad = angle_min1 + closest_range_seq*delta_scan;
    // }
    // else
    // {
    //     closest_range_ = closest_range_pos_;
    //     auto closest_range_seq = Arrs2.end() - closest_range_pos;
    //     closest_degree_rad = angle_max1 - closest_range_seq*delta_scan;
    // }
    // // changed
    // if((0.785 < closest_degree_rad)&&(closest_degree_rad < 5.495))
    // {
    //     closest_range_ = 3.5;
    // }
    //float closest_degree = rad2deg(closest_degree_rad);

    _closest_distance = closest_range_;
    _closest_degree = closest_degree_rad;

    // 显示
    // std::cout << "angle_min: " << angle_min1 << std::endl;
    // std::cout << "angle_max: " << angle_max1 << std::endl;
    // // std::cout << "Sample number: " << len_ <<std::endl;
    // std::cout << "Seq number: " << closest_range_seq <<std::endl;
    // std::cout << "最短距离：" << _closest_distance << std::endl;
    // std::cout << "弧度：" << _closest_degree << std::endl;
    
}
void shnMPCNode::Generate_Usr_Command(double linear, double angular)
{
    _sub_twist_cmd.linear.x = linear;
    _sub_twist_cmd.angular.z = angular;
}
void shnMPCNode::Generate_Robot_Command(double linear, double angular)
{
    _robot_twist_cmd.linear.x = linear;
    _robot_twist_cmd.angular.z = angular;
}

/*****************/
/* MAIN FUNCTION */
/*****************/

int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "LoopControlNode");

    shnMPCNode mpc_node;
    
    ros::AsyncSpinner spinner(mpc_node.get_thread_numbers()); // Use multi threads
    spinner.start();
    ros::waitForShutdown();
    return 0;
}